#include "StdAfx.h"
#include <stdio.h>
#include "LeapFrogIntegrator.h"

CLeapFrogIntegrator::CLeapFrogIntegrator(int nVarCount)
: m_dStep(0.0001)
, m_nVarCount(nVarCount)
{
	m_vecForce.resize(m_nVarCount, 0.0);
}

bool CLeapFrogIntegrator::Move(double tStart, double tEnd, double *x, double *v, IForceInterface *pForce)
{
	if(pForce == NULL)
	{
		ASSERT(0);
		return false;
	}

	if(tStart == tEnd)
		return true;

	bool bMoveForvard = ((tEnd - tStart) > 0.0)? true: false;
	double * f = &m_vecForce[0];

	while(1)
	{
		double step = tEnd - tStart;

		bool bDone = (bMoveForvard? (step <= 0.0): (step >= 0.0))? true: false;
		if(bDone)
			break;

		step = min(step, m_dStep);
		step = max(step, -m_dStep);
		double dHalfStep = step * 0.5;

		pForce->Force(m_nVarCount, tStart, x, f);
		for(int i=0; i<m_nVarCount; ++i)
		{
			v[i] += f[i] * dHalfStep;
			x[i] += v[i] * step;
		}
		pForce->Force(m_nVarCount, tStart, x, f);
		for(int i=0; i<m_nVarCount; ++i)
		{
			v[i] += f[i] * dHalfStep;
		}
		_ASSERT(tStart + step != tStart);
		tStart += step;
		//printf("%lf\n", ToDouble(tStart));
	}

	return true;
}


